import network
import time
import _thread
import collections
import socket
from havi import ports

# ===== WiFi =====
SSID = "WiFi network name (Case sensitive)"
PASSWORD = "WiFi password "

wifi = network.WLAN(network.STA_IF)
wifi.active(True)
wifi.disconnect()
wifi.connect(SSID, PASSWORD)

print("Connecting to WiFi...")
for _ in range(20):
    if wifi.isconnected():
        break
    print(".", end="")
    time.sleep(1)
print()
if wifi.isconnected():
    print("Connected:", wifi.ifconfig())
else:
    print("Failed to connect to WiFi")

# ===== Hardware =====
servo = ports.servo1()
ultrasonic_front = ports.input1("ultrasonic sensor")   # ✅ use Havi ports

# ===== Shared state =====
current_angle = 0
current_distance = 0
last_valid_distance = 0
distance_ok = 1   # 1=valid, 0=error, 2=no reading, -1=out of range
dist_history = collections.deque([], 3)
sweep_delay = 0.03   # seconds per step
MAX_RANGE = 100      # ✅ set SONAR range to 100 cm

# ===== Servo sweep thread =====
def servo_sweep():
    global current_angle, sweep_delay
    min_angle, max_angle = 5, 175
    step, direction = 1, 1
    while True:
        for ang in range(min_angle, max_angle + 1, step):
            if direction == -1:
                ang = max_angle - (ang - min_angle)
            try:
                servo.move(ang)
            except:
                pass
            current_angle = ang
            time.sleep(sweep_delay)
        direction *= -1

# ===== Distance reading thread =====
def distance_loop():
    global current_distance, last_valid_distance, distance_ok
    decay = 0.98
    while True:
        try:
            d = ultrasonic_front.distance_cm()
            if d is not None and 2 < d <= MAX_RANGE:
                dist_history.append(d)
                sv = sorted(dist_history)
                mid = len(sv) // 2
                current_distance = int(sv[mid])
                last_valid_distance = current_distance
                distance_ok = 1   # 🟢 valid
            elif d is not None and d > MAX_RANGE:
                current_distance = MAX_RANGE
                distance_ok = -1  # out of range → no blip
            else:
                last_valid_distance = int(last_valid_distance * decay)
                current_distance = last_valid_distance
                distance_ok = 2   # 🔵 no reading
        except:
            last_valid_distance = int(last_valid_distance * decay)
            current_distance = last_valid_distance
            distance_ok = 2
        time.sleep(0.03)

# start threads
_thread.start_new_thread(servo_sweep, ())
_thread.start_new_thread(distance_loop, ())

# ===== HTML UI =====
html = """<!doctype html>
<html><head><meta charset="utf-8"><meta name="viewport" content="width=device-width,initial-scale=1">
<title>SONAR 180°</title>
<style>
body{background:#000;color:#9ff49a;font-family:monospace;text-align:center}
canvas{display:block;margin:12px auto;position:relative}
#info{margin-top:6px}
#statusdot{width:14px;height:14px;border-radius:50%;display:inline-block;margin-left:8px;background:#555;}
#ctrl{margin-top:10px;color:#9ff49a}
#legend{position:absolute;bottom:10px;left:10px;text-align:left;color:#9ff49a;font-size:14px;}
#legend div{margin:2px 0;}
input[type=range]{width:200px}
</style>
</head><body>
<h2>SONAR 180° <span id="statusdot"></span></h2>
<div style="position:relative;width:520px;margin:auto;">
<canvas id="rad" width="520" height="520"></canvas>
<div id="legend">
  <div><span style="color:#0f0;">●</span> Valid</div>
  <div><span style="color:#f00;">●</span> Error</div>
  <div><span style="color:#00f;">●</span> No Reading</div>
</div>
</div>
<div id="info">Angle: -- | Dist: -- cm</div>
<div id="ctrl">
Sweep delay: <span id="delayval">30</span> ms<br>
<input type="range" id="delay" min="5" max="200" value="30">
</div>
<script>
const c=document.getElementById('rad'),ctx=c.getContext('2d'),W=c.width,H=c.height,CX=W/2,CY=H/2,R=Math.min(W,H)*0.42,MAX=100;
const dot=document.getElementById('statusdot');
let lastUpdate=Date.now();
let points=[];let sweepAngle=0;

function drawBg(){
  ctx.fillStyle='rgba(0,0,0,0.25)';ctx.fillRect(0,0,W,H);
  ctx.font='14px monospace';
  for(let i=1;i<=4;i++){let r=R*(i/4);ctx.beginPath();ctx.arc(CX,CY,r,0,2*Math.PI);ctx.strokeStyle='rgba(0,255,160,0.18)';ctx.stroke();ctx.fillStyle='rgba(160,255,200,0.95)';ctx.fillText((25*i)+' cm',CX+6,CY-r+16);}
  for(let a=0;a<360;a+=30){let rad=a*Math.PI/180,x=CX+Math.cos(rad)*R,y=CY+Math.sin(rad)*R;ctx.beginPath();ctx.moveTo(CX,CY);ctx.lineTo(x,y);ctx.strokeStyle='rgba(0,255,120,0.08)';ctx.stroke();if(a%90===0)ctx.fillText(a+'°',x,y);}
  ctx.beginPath();ctx.arc(CX,CY,4,0,2*Math.PI);ctx.fill();
}
function drawSweep(){let rad=sweepAngle*Math.PI/180;let x=CX+Math.cos(rad)*R,y=CY+Math.sin(rad)*R;ctx.strokeStyle='rgba(0,255,0,0.6)';ctx.beginPath();ctx.moveTo(CX,CY);ctx.lineTo(x,y);ctx.stroke();}
function drawBlips(){
  let now=Date.now();
  points=points.filter(p=>now-p.t<4000);
  for(let p of points){
    let age=(now-p.t)/4000;
    let alpha=1-age;
    if(p.ok===1){ ctx.fillStyle='rgba(0,255,0,'+alpha+')'; }   // green
    else if(p.ok===0){ ctx.fillStyle='rgba(255,0,0,'+alpha+')'; } // red
    else if(p.ok===2){ ctx.fillStyle='rgba(0,0,255,'+alpha+')'; } // blue
    else { continue; } // out of range (-1) → skip
    ctx.beginPath();
    ctx.arc(p.x,p.y,5,0,2*Math.PI);
    ctx.fill();
  }
}
function addReading(angle,dist,ok){
  sweepAngle=angle;
  let safe=Math.max(0,Math.min(dist||0,MAX));
  let rad=angle*Math.PI/180;
  let r=(safe/MAX)*R;
  let x=CX+Math.cos(rad)*r,y=CY+Math.sin(rad)*r;
  if(ok !== -1){ // skip out of range
    points.push({x,y,t:Date.now(),ok:ok});
  }
  document.getElementById('info').innerText='Angle: '+angle+'° | Dist: '+dist+' cm';
  lastUpdate=Date.now();
  if(ok===1){ dot.style.background='#0f0'; }
  else if(ok===2){ dot.style.background='#00f'; }
  else if(ok===0){ dot.style.background='#f00'; }
  else { dot.style.background='#555'; }
  drawFrame();
}
function drawFrame(){drawBg();drawSweep();drawBlips();}
async function poll(){try{let res=await fetch('/data');let t=await res.text();let [a,d,ok]=t.split(',');addReading(parseInt(a)||0,parseInt(d)||0,parseInt(ok)||0);}catch(e){}setTimeout(poll,60);}
setInterval(()=>{if(Date.now()-lastUpdate>200){dot.style.background='#f00';}},100);
drawBg();poll();

// sweep delay control
const slider=document.getElementById('delay'),disp=document.getElementById('delayval');
slider.addEventListener('input',async e=>{
  let v=slider.value;
  disp.innerText=v;
  try{await fetch('/speed?val='+v);}catch(err){}
});
</script>
</body></html>
"""

# ===== Simple HTTP server =====
addr = socket.getaddrinfo("0.0.0.0", 80)[0][-1]
s = socket.socket()
s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
s.bind(addr)
s.listen(1)
s.settimeout(0.2)
print("HTTP server listening on", addr)

while True:
    try:
        try:
            cl, a = s.accept()
        except OSError:
            continue
        req = cl.recv(1024).decode()
        if req.startswith("GET /data"):
            cl.send("HTTP/1.0 200 OK\r\nContent-type: text/plain\r\n\r\n")
            cl.send("%d,%d,%d" % (current_angle, current_distance, distance_ok))
            cl.close()
        elif req.startswith("GET /speed"):
            try:
                val = req.split("val=")[1].split(" ")[0]
                ms = max(5, min(200, int(val)))
                sweep_delay = ms / 1000.0
                print("Sweep delay set to", sweep_delay, "seconds")
                cl.send("HTTP/1.0 200 OK\r\nContent-type: text/plain\r\n\r\nOK")
            except Exception as e:
                print("Speed change error:", e)
                cl.send("HTTP/1.0 400 Bad Request\r\n\r\nERR")
            cl.close()
        else:
            cl.send("HTTP/1.0 200 OK\r\nContent-type: text/html\r\n\r\n")
            cl.send(html)
            cl.close()
    except Exception as e:
        print("HTTP server loop error:", e)

